function [p1,p2,g1,g2] = doublePendulumPosition(z,P) 
%[p1,p2,g1,g2] = DOUBLEPENDULUMPOSITION(Z,P)
% 
%FUNCTION:  This function computes the energy of a double
%    pendulum.
%INPUTS: 
%    z = [4xn] matrix of states.
%    P = struct of parameters
%OUTPUTS: 
%    p1 = [2xn] position of the end of the first link
%    p2 = [2xn] position of the end of the second link
%    g1 = [2xn] position of the CoM of link one
%    g2 = [2xn] position of the CoM of link two
% 
%NOTES:
%    This file was automatically generated by writeDoublePendulumPosition

m1 = P.m1; %link one mass
m2 = P.m2; %link two mass
g  = P.g ; %gravity
l1 = P.l1; %link one length
l2 = P.l2; %link two length
I1 = P.I1; %link one moment of inertia about its center of mass
I2 = P.I2; %link two moment of inertia about its center of mass
d1 = P.d1; %distance between link one center of mass and parent joint
d2 = P.d2; %distance between link two center of mass and parent joint

th1 = z(1,:); %link one absolute angle
dth1 = z(2,:); %link one angular rate
th2 = z(3,:); %link two absolute angle
dth2 = z(4,:); %link two angular rate

n = length(th1); 
p1 = zeros(2,n);
p2 = zeros(2,n);
g1 = zeros(2,n);
g2 = zeros(2,n);
p1(1,:) = l1.*cos(th1);
p1(2,:) = l1.*sin(th1);
p2(1,:) = l1.*cos(th1) + l2.*cos(th2);
p2(2,:) = l1.*sin(th1) + l2.*sin(th2);
g1(1,:) = d1.*cos(th1);
g1(2,:) = d1.*sin(th1);
g2(1,:) = d2.*cos(th2) + l1.*cos(th1);
g2(2,:) = d2.*sin(th2) + l1.*sin(th1);

end 
